// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GeometryManipulator.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Visualization
{
    using System.IO;
    using System.Windows.Media;
    using System.Windows.Media.Media3D;

    using HelixToolkit.Wpf;

    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// A class that manipulates 3D geometries
    /// </summary>
    public class GeometryManipulator
    {
        /// <summary>
        /// A red brush
        /// </summary>
        private static DiffuseMaterial redBrush = new DiffuseMaterial(new SolidColorBrush(Colors.Red));

        /// <summary>
        /// A green brush
        /// </summary>
        private static DiffuseMaterial greenBrush = new DiffuseMaterial(new SolidColorBrush(Colors.Green));

        /// <summary>
        /// A blue brush
        /// </summary>
        private static DiffuseMaterial blueBrush = new DiffuseMaterial(new SolidColorBrush(Colors.Blue));

        /// <summary>
        /// Load an geometry
        /// </summary>
        /// <param name="name">Name for the geometry</param>
        /// <param name="path">Geometry path</param>
        /// <returns>A geometry model</returns>
        public static Model3D Load3DModel(string name, string path)
        {
            ModelImporter mi = new ModelImporter();
            return mi.Load(path);
        }

        /// <summary>
        /// Load an geometry
        /// </summary>
        /// <param name="name">Name for the geometry</param>
        /// <param name="path">Geometry path</param>
        /// <param name="color">Color of the geometry</param>
        /// <returns>A geometry model</returns>
        public static GeometryModel3D Load3DModel(string name, string path, Color color)
        {
            ModelImporter importer = new ModelImporter();
            Model3DGroup piece = importer.Load(path);
            GeometryModel3D model = piece.Children[0] as GeometryModel3D;
            Material material = MaterialHelper.CreateMaterial(color);
            GeometryModel3D geo = new GeometryModel3D(model.Geometry, material);
            return geo;
        }

        /// <summary>
        /// Create a frame model
        /// </summary>
        /// <param name="frame">A frame</param>
        /// <param name="scale">Scale of the length of the frame. The value is chosen such that
        /// the visualized frame is in the ideal proportion to other visualized components. In current
        /// implementation, if the geometry is defined in meters, 0.1 seems a reasonable scale.</param>
        /// <param name="diameter">Scale of the diameter of the cylinder used to represent an arrow/axis</param>
        /// <returns>A model</returns>
        public static Model3D CreateAFrame(Matrix4 frame, double scale = 0.1, double diameter = 0.01)
        {
            Model3DGroup frameModel = new Model3DGroup();
            MeshBuilder builderX = new MeshBuilder();
            MeshBuilder builderY = new MeshBuilder();
            MeshBuilder builderZ = new MeshBuilder();

            Point3D origin = new Point3D(frame[0, 3], frame[1, 3], frame[2, 3]);
            Point3D pointOnX = new Point3D(origin.X + frame[0, 0] * scale, origin.Y + frame[1, 0] * scale, origin.Z + frame[2, 0] * scale);
            Point3D pointOnY = new Point3D(origin.X + frame[0, 1] * scale, origin.Y + frame[1, 1] * scale, origin.Z + frame[2, 1] * scale);
            Point3D pointOnZ = new Point3D(origin.X + frame[0, 2] * scale, origin.Y + frame[1, 2] * scale, origin.Z + frame[2, 2] * scale);

            builderX.AddArrow(origin, pointOnX, diameter);
            builderY.AddArrow(origin, pointOnY, diameter);
            builderZ.AddArrow(origin, pointOnZ, diameter);

            MeshGeometry3D meshX = builderX.ToMesh();
            MeshGeometry3D meshY = builderY.ToMesh();
            MeshGeometry3D meshZ = builderZ.ToMesh();

            GeometryModel3D geometryXAxis = new GeometryModel3D(meshX, redBrush);
            GeometryModel3D geometryYAxis = new GeometryModel3D(meshY, greenBrush);
            GeometryModel3D geometryZAxis = new GeometryModel3D(meshZ, blueBrush);

            frameModel.Children.Add(geometryXAxis);
            frameModel.Children.Add(geometryYAxis);
            frameModel.Children.Add(geometryZAxis);

            return frameModel;
        }

        /// <summary>
        /// Create an arrow
        /// </summary>
        /// <param name="origin">Origin of the arrow</param>
        /// <param name="pointTo">One point it is pointing to</param>
        /// <param name="diameter">Diameter of the arrow</param>
        /// <param name="color">Color of the arrow</param>
        /// <returns>An arrow</returns>
        public static Model3D CreateAnArrow(Point3D origin, Point3D pointTo, double diameter, Color color)
        {
            MeshBuilder builder = new MeshBuilder();

            builder.AddArrow(origin, pointTo, diameter);
            MeshGeometry3D mesh = builder.ToMesh();
            DiffuseMaterial colorBrush = new DiffuseMaterial(new SolidColorBrush(color));
            GeometryModel3D geometryArrow = new GeometryModel3D(mesh, colorBrush);

            return geometryArrow;
        }

        /// <summary>
        /// Create a sphere
        /// </summary>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius of the sphere</param>
        /// <param name="color">Color of the sphere</param>
        /// <returns>A sphere</returns>
        public static Model3D CreateASphere(Point3D center, double radius, Color color)
        {
            MeshBuilder builder = new MeshBuilder();

            builder.AddSphere(center, radius);
            MeshGeometry3D mesh = builder.ToMesh();
            DiffuseMaterial redBrush = new DiffuseMaterial(new SolidColorBrush(color));
            GeometryModel3D geometrySphere = new GeometryModel3D(mesh, redBrush);

            return geometrySphere;
        }
    }
}
